/***************************************************************************
 *   Copyright (C) 2010 by Oleg Goncharov  *
 *   $EMAIL$                           *                          
 *                                                                         *
 *   This file is part of ChessVision.                                     *
 *                                                                         *
 *   ChessVision is free software; you can redistribute it and/or modify   *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <iostream>
#include <opencv/cv.h>

#include "error/cerror.h"
#include "vision/cimagefromfile.h"
#include "vision/cimagecamthread.h"
#include "vision/cimagedvgrab.h"
#include "vision/cpointmaxcolor.h"
#include "vision/ccamtransform.h"
#include "vision/cpointcirclecenter.h"
#include "vision/cpointmatch.h"
#include "vision/pointpyrmatch.h"

#include "katanavision/ckatanawitheye.h"
#include "katanavision/katanafuncs.h"

using namespace std;
using namespace Katana;

cv::Point2f target;

void on_mouse( int event, int x, int y, int flags, void* param )
{
	switch( event )
	{
		case CV_EVENT_LBUTTONUP:
			target.x = x;
			target.y = y;	
			break;
	}
}

//5 : KATANA + CAMERA CALIBRATION
int main( int argc, char *argv[] )
{	
	TCdlCOMDesc ccd = {0, 57600, 8, 'N', 1, 300, 0};
	CCdlCOM device(ccd);
	CCplSerialCRC protocol;
	Katana::CKatanaSensing katana;
	
	
	try {
		protocol.init(&device);
		katana.create ("katana5M180.cfg", &protocol );
		
		cvNamedWindow("win", 1);
		cvSetMouseCallback( "win", on_mouse, 0 );
		
		//CImageCamera cam(0);
		CImageCamThread cam(0, 50);
		cvShowImage("win", cam.Img());
		
		
		IplImage * endarm_sample = cvLoadImage("pattern.jpg");
		if (!endarm_sample) throw Errors::CError("No pattern");
		IplImage * endarm_test = cvLoadImage("test1.jpg");
		if (!endarm_test) throw Errors::CError("No pattern test image");
		CPointScaleMatch endarm(endarm_sample, 0.5, 2.0, 4, CPointNorm::BRIGHTNESS_NORM);
		//CPointPyrMatch endarm(cv::Mat(endarm_sample), cv::Mat(endarm_test), 0.90);
		//CPointScaleMatch endarm(endarm_sample, 0.5, 2.0, 4, CPointNorm::NO_NORM);
		
 		CCamTransform camtr("calibdata.xml");

		
		Katana::CKatanaWithEye arm(katana, cam, endarm, camtr);	
		katana.closeGripper(true);
		
		arm.Calibrate();
		//camtr.LoadTransform("transform.xml");
		camtr.PrintIParams();
		camtr.PrintTParams();
		camtr.SaveTransform("transform.xml");
		
		arm.movToImagePoint(cv::Point2f(320, 240), HPlane3f(0,0,1,100), 0, true);
		
		while (1) {
			cam.GrabFrame();
			cvCircle(cam.Img(), target, 4, cvScalar(0,255,0), -1);
			cvShowImage("win", cam.Img());
			cvWaitKey(0);
			arm.movToImagePoint(target, HPlane3f(0,0,1,100), 0, true);
			//arm.CheckTransform(5, false);
		}
	} 
	catch ( Exception &e ) {
		katana.freezeRobot();
		std::cout << "ERROR: " << e.message() << std::endl;
		return -1;
	}
	catch ( Errors::CError& e ) {
		e.Report(std::cout);
	}
}
